% [X, CovX, x0, Covx0, CovX_X, entropy] = rts_smoother(x0, Covx0, X, CovX, A, Q)
%
% Rauch-Tung-Striebel smoother
%
% Use after applying Kalman filter.
%
% In addition, returns the entropy of the posterior distribution.

% Last modified 2011-10-19
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@aalto.fi)

function [X, CovX, x0, Covx0, CovX_X, entropy] = rts_smoother(x0, Covx0, X, CovX, A, Q)

N = size(X,2);

if nargout >= 6
  entropy = gaussian_entropy(logdet_cov(CovX(:,:,N)), size(CovX,1));
end

% Smooth the signal
for n=(N-1):-1:1
  if nargout <= 4
    [X(:,n), CovX(:,:,n)] = ...
        rts_smoother_step(X(:,n), ...
                          CovX(:,:,n), ...
                          X(:,n+1), ...
                          CovX(:,:,n+1), ...
                          A, ...
                          Q);
  elseif nargout <= 5
    [X(:,n), CovX(:,:,n), CovX_X(:,:,n+1)] = ...
        rts_smoother_step(X(:,n), ...
                          CovX(:,:,n), ...
                          X(:,n+1), ...
                          CovX(:,:,n+1), ...
                          A, ...
                          Q);
  else
    [X(:,n), CovX(:,:,n), CovX_X(:,:,n+1), ent] = ...
        rts_smoother_step(X(:,n), ...
                          CovX(:,:,n), ...
                          X(:,n+1), ...
                          CovX(:,:,n+1), ...
                          A, ...
                          Q);
    entropy = entropy + ent;
  end
end

% Smooth the initial state
if nargout <= 4
  [x0, Covx0] = ...
      rts_smoother_step(x0, ...
                        Covx0, ...
                        X(:,1), ...
                        CovX(:,:,1), ...
                        A, ...
                        Q);
elseif nargout <= 5
  [x0, Covx0, CovX_X(:,:,1)] = ...
      rts_smoother_step(x0, ...
                        Covx0, ...
                        X(:,1), ...
                        CovX(:,:,1), ...
                        A, ...
                        Q);
else
  [x0, Covx0, CovX_X(:,:,1), ent] = ...
      rts_smoother_step(x0, ...
                        Covx0, ...
                        X(:,1), ...
                        CovX(:,:,1), ...
                        A, ...
                        Q);
  entropy = entropy + ent;
end



